/*
 * @Author: liu Shihao
 * @Date: 2022-06-12 20:54:47
 * @LastEditors: liu Shihao
 * @LastEditTime: 2022-06-13 19:22:38
 * @FilePath: \bearpi-hm_nano\applications\BearPi\BearPi-HM_Nano\sample\robot\include\scs15_control.h
 * @Description: 空闲中断
 * Copyright (c) 2022 by ${fzu} email: logic_fzu@outlook.com, All Rights Reserved.
 */

#ifndef __ROBOT_BODY_H
#define __ROBOT_BODY_H
#include <stdio.h>
#include <string.h>
#include "stdint.h"

#define BASE_HIGH     160.0f
#define LONG_LENGTH   250.0f
#define SHORT_LENGTH  180.0f
#define THIGH  110.0f     //大腿 长度
#define SHANK  100.0f    //前小腿 长度
#define HIP     45.0f    //髋关节长度
#define pi       3.1415926f	 
#define Angle_to_Encoder 4.654f   //
#define Rad_to_Degree    57.29578f


struct Leg_t
{
	uint8_t IDs[3];
	int position[3];
    int bias_position[3];
	int coefficient_1;
	int coefficient_2;
	int coefficient_3;
	int  vmc_flag;
	float x;
	float y;
	float z;
	float pos_x;
	float pos_y;
	float pos_z;
	float delta_x;
	float delta_y;
	float delta_z;
	float delta_xy;
	float angle_1;	//踝关节电机 立体面中角c
	float angle_2;	//大腿关节   平面角a
 	float angle_3;	//小腿关节   平面角b 
	float count;
	enum
	{
		Down,
		Up,
	} state,last_state;
};


struct Body_t
{
	int vx;
	int vy;
	int rotate;
	int vk;
	float yaw;
	float pitch;
	float roll;
	float last_yaw;
	float last_pitch;
	float last_roll;
	enum
	{
		Walk,
		Stop,
		Attitude,
        Attitude_style,
		Leftturn,
		Rightturn,
		translation,
		Body_Stable,
		Turn_off,
		Turn_on,
		Line_Trot,
		Line_Walk
	} workstate;
};
extern struct Body_t body;


void BodyInit(void);
void LegChange(void);
void ServoSendData(void);
void BodyChange(void);







#endif  //__ROBOT_BODY_H



